# ==============================================================================
# Copyright (c) 2025 CompanyName. All rights reserved.
# Author:         22020873 陈泽欣
# Project:        Design of Deep Learning Fundamental Course
# Module:         pnp_solver.py
# Date:           2025-05-24
# Description:    本模块封装了基于 PnP（Perspective-n-Point）算法的位姿求解功能，
#                 主要用于从检测到的 2D 图像点与已知的骰子 3D 模型点中估计其旋转和平移向量。
#                 
#                 核心功能包括：
#                 - 使用 OpenCV 的 solvePnP 函数进行初始位姿估计；
#                 - 计算重投影误差以评估估计精度；
#                 是整个骰子姿态识别系统中视觉几何计算与位姿初始化的关键组件。
# ==============================================================================

import cv2
from configs.norm_data import *

def solve_pnp(object_2d):
    object_2d_point = np.array(object_2d, dtype=np.double)

    found, rvec, tvec = cv2.solvePnP(dice_3d_points[4:8], object_2d_point, camera_matrix, dist_coefs, flags=0)

    return rvec, tvec


def calculate_reprojection_error(observation_point, imgpts):
    error = 0
    for i in range(len(observation_point)):
        p1 = np.array(observation_point[i], dtype=np.int32)
        p2 = np.array(imgpts[i + 4].ravel(), dtype=np.int32)
        error += cv2.norm(p1, p2, cv2.NORM_L2) / len(observation_point)
    return error
    